#!/usr/bin/env python
import roslib; roslib.load_manifest('facedetect')
import rospy

from sensor_msgs.msg import Image

import cv
from cv_bridge import CvBridge, CvBridgeError

FRONTAL_FACE = roslib.packages.find_resource('opencv2', 'haarcascade_frontalface_alt.xml')[0]
LEFT_EYE = roslib.packages.find_resource('opencv2', 'haarcascade_lefteye_2splits.xml')[0]
RIGHT_EYE = roslib.packages.find_resource('opencv2', 'haarcascade_righteye_2splits.xml')[0]


class EyeDetector():
    def __init__(self):
        rospy.init_node('eye_detector')

        cv.NamedWindow("Faces", 1)
        self.bridge = CvBridge()
        self.imageSub = rospy.Subscriber("/logitech_usb_webcam/image_raw", Image, self.handle_image)
        self.face_cascade = cv.Load(FRONTAL_FACE)
        self.left_eye_cascade = cv.Load(LEFT_EYE)
        self.right_eye_cascade = cv.Load(RIGHT_EYE)

    def draw_rect(self, cv_image, rect):
        cv.Rectangle(cv_image, (rect[0], rect[1]), (rect[0] + rect[2], rect[1] + rect[3]), (0, 255, 0, 0), 2)
                

    def handle_image(self, img_msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv(img_msg, desired_encoding="passthrough")
            
            face_result = cv.HaarDetectObjects(cv_image, self.face_cascade, cv.CreateMemStorage(0), 1.2, 2, 0)
            if len(face_result) > 0:    
                face_rect = face_result[0][0]
                self.draw_rect(cv_image, face_rect)

            left_eye_result = cv.HaarDetectObjects(cv_image, self.left_eye_cascade, cv.CreateMemStorage(0), 1.2, 2, 0)
            if len(left_eye_result) > 0:             
                left_eye_rect = left_eye_result[0][0]
                self.draw_rect(cv_image, left_eye_rect)

            right_eye_result = cv.HaarDetectObjects(cv_image, self.right_eye_cascade, cv.CreateMemStorage(0), 1.2, 2, 0)
            if len(right_eye_result) > 0:             
                right_eye_rect = right_eye_result[0][0]
                self.draw_rect(cv_image, right_eye_rect)
                    
            cv.ShowImage("Faces", cv_image)
            cv.WaitKey(3)

        except CvBridgeError, e:
            print e

if __name__ == '__main__':    
    try:
        eye = EyeDetector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
    cv.DestroyAllWindows()
